% 2024/03/25
% Author: LHP
% Description: diff drive robot model defined by CasADi

function [model, constraint] = diff_drive_robot_model()

    import casadi.*
    model = struct();
    constraint = struct();

    model_name = 'diff_drive_robot_model';

%% CasADi Model

    nx = 3;
    nu = 2;

    % 定义符号变量
    % 定义状态向量：x方向位置/y方向位置/姿态
    X = SX.sym('X',nx,1);               % 状态向量
    Xr = SX.sym('Xr', nx, 1);           % 参考状态:Xr=[xr]

    % 定义控制输入向量：线速度/角速度
   
    u = SX.sym('u',nu,1);            % 输入向量:u=[u;u_prev]

    % 创建状态变量的导数
    Xdot = SX.sym('Xdot', nx, 1); 

    % 定义系统模型
    % 显式动态方程f_expl
    f_expl = vertcat(...
        u(1,1) * cos(X(3)),...
        u(1,1) * sin(X(3)),...
        u(2,1)...
    ); 

    % 代价权重矩阵
    Q = diag([1.0, 1.0, 1.0]);                             % 状态权重矩阵
    R = eye(nu);                                           % 输入权重矩阵
    R(1, 1) = 1.0; R(2, 2) = 1.0;
    Qe = Q;                                                % 终端状态权重矩阵

    %W   = blkdiag(Q, R);                                  % W是一个对角矩阵，包含Q和R的对角线部分，用于加权状态和输入的误差
    %W_e = Qe;

    % 定义约束
    % 通用代价函数形式
    cost_expr_ext_cost_e = (X-Xr)'*Q*(X-Xr);               % 终端代价 (only states)
    cost_expr_ext_cost   = cost_expr_ext_cost_e;% + u'*R*u;% + deltau'*0.5*R*deltau;  % 过程代价 (states and inputs)

    % 控制输入约束
    model.v_min     = -0.5;                                % 线速度的最小值[m/s]
    model.v_max     = 0.5;                                 % 线速度的最大值 [m/s]
    model.omega_min = -0.5;                                % 角速度的最小值[rad/s]
    model.omega_max = 0.5;                                 % 角速度的最大值[rad/s]

    % Define initial conditions
    model.x0 = [0; 0; pi/2];                               % 设置初始状态条件

    % Define model struct
    %model.f_impl_expr = f_expl - Xdot;                    % 设置隐式动态方程
    model.f_expl_expr = f_expl;                            % 设置显式动态方程
    model.x           = X;                                 % 设置状态变量
    model.xdot        = Xdot;                              % 设置状态变量的导数
    model.u           = u;                                 % 设置控制变量
    model.p           = Xr;                                % 设置参考状态变量
    model.name        = model_name;                        % 设置模型名称

    %model.W = W;
    %model.W_e = W_e;

    model.cost_expr_ext_cost = cost_expr_ext_cost;
    model.cost_expr_ext_cost_e = cost_expr_ext_cost_e;
end